/******************************************************************************/
/*  mb_func_holding.c                                                         */
/*                                                                            */
/*  FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. */
/*  Copyright (c) 2006 Christian Walter <wolti@sil.at>                        */
/*  All rights reserved.                                                      */
/******************************************************************************/


#include "data_types.h"

#include "port\port.h"
#include "mbconfig.h"
#include "mb.h"
#include "mbconfig.h"
#include "mbframe.h"
#include "mbproto.h"
#include "functions\mbutils.h"
#include "mbfunc.h"


/******************************************************************************/
/*      Defines                                                               */
/******************************************************************************/
#define MB_PDU_FUNC_READ_ADDR_OFF               (MB_PDU_DATA_OFF + 0)
#define MB_PDU_FUNC_READ_REGCNT_OFF             (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_READ_SIZE                   (4)
#define MB_PDU_FUNC_READ_REGCNT_MAX             (0x007D)

#define MB_PDU_FUNC_WRITE_ADDR_OFF              (MB_PDU_DATA_OFF + 0)
#define MB_PDU_FUNC_WRITE_VALUE_OFF             (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_WRITE_SIZE                  (4)

#define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF          (MB_PDU_DATA_OFF + 0)
#define MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF        (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF       (MB_PDU_DATA_OFF + 4)
#define MB_PDU_FUNC_WRITE_MUL_VALUES_OFF        (MB_PDU_DATA_OFF + 5)
#define MB_PDU_FUNC_WRITE_MUL_SIZE_MIN          (5)
#define MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX        (0x0078)

#define MB_PDU_FUNC_READWRITE_READ_ADDR_OFF     (MB_PDU_DATA_OFF + 0)
#define MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF   (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF    (MB_PDU_DATA_OFF + 4)
#define MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF  (MB_PDU_DATA_OFF + 6)
#define MB_PDU_FUNC_READWRITE_BYTECNT_OFF       (MB_PDU_DATA_OFF + 8)
#define MB_PDU_FUNC_READWRITE_WRITE_VALUES_OFF  (MB_PDU_DATA_OFF + 9)
#define MB_PDU_FUNC_READWRITE_SIZE_MIN          (9)



eMB_EXCEPTION prveMbError2Exception(const eMB_ERROR_CODE _errorCode);



#if MB_FUNC_WRITE_HOLDING_ENABLED > 0
//==============================================================================
// MbFuncWriteHoldingRegister()
//==============================================================================
eMB_EXCEPTION MbFuncWriteHoldingRegister(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if (*_pLen == (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
        regAddress++;

        // Make callback to update the value
        regStatus = ModbusRegHoldingCb(&_pFrame[MB_PDU_FUNC_WRITE_VALUE_OFF],
                                       regAddress, 1, MB_REG_WRITE);

        // If an error occured convert it into a Modbus exception
        if (regStatus != MB_ENOERR) {
            status = prveMbError2Exception(regStatus);
        };
    }
    else {
        // Can't be a valid request because the length is incorrect
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif


#if MB_FUNC_WRITE_MULTIPLE_HOLDING_ENABLED > 0
//==============================================================================
// MbFuncWriteMultipleHoldingRegister()
//==============================================================================
eMB_EXCEPTION MbFuncWriteMultipleHoldingRegister(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint16_t regCount;
    uint8_t regByteCount;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if (*_pLen >= (MB_PDU_FUNC_WRITE_MUL_SIZE_MIN + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1]);
        regAddress++;

        regCount = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF] << 8);
        regCount |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF + 1]);

        regByteCount = _pFrame[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF];

        if ((regCount >= 1)
            && (regCount <= MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX)
                && (regByteCount == (uint8_t) (2 * regCount)))
        {
            // Make callback to update the register values
            regStatus = ModbusRegHoldingCb(&_pFrame[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF],
                                           regAddress, regCount, MB_REG_WRITE);

            // If an error occured convert it into a Modbus exception
            if (regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            }
            else {
                /*  The response contains the function code, the starting
                 *  address and the quantity of registers. We reuse the
                 *  old values in the buffer because they are still valid.
                 */
                *_pLen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF;
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid request because the length is incorrect
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif


#if MB_FUNC_READ_HOLDING_ENABLED > 0
//==============================================================================
// MbFuncReadHoldingRegister()
//==============================================================================
eMB_EXCEPTION MbFuncReadHoldingRegister(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint16_t regCount;
    uint8_t * pFrameCur;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if (*_pLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
        regAddress++;

        regCount = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_REGCNT_OFF] << 8);
        regCount = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_REGCNT_OFF + 1]);

        // Check if the number of registers to read is valid. If not
        // return Modbus illegal data value exception. 
        if ((regCount >= 1) && (regCount <= MB_PDU_FUNC_READ_REGCNT_MAX)) {
            // Set the current PDU data pointer to the beginning
            pFrameCur = &_pFrame[MB_PDU_FUNC_OFF];
            *_pLen = MB_PDU_FUNC_OFF;

            // First byte contains the function code
            *pFrameCur++ = MB_FUNC_READ_HOLDING_REGISTER;
            *_pLen += 1;

            // Second byte in the response contain the number of bytes
            *pFrameCur++ = (uint8_t) (regCount * 2);
            *_pLen += 1;

            // Make callback to fill the buffer
            regStatus = ModbusRegHoldingCb(pFrameCur, regAddress, regCount, MB_REG_READ);

            // If an error occured convert it into a Modbus exception
            if (regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            }
            else {
                *_pLen += regCount * 2;
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid request because the length is incorrect
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif


#if MB_FUNC_READWRITE_HOLDING_ENABLED > 0
//==============================================================================
// MbFuncReadWriteMultipleHoldingRegister()
//==============================================================================
eMB_EXCEPTION MbFuncReadWriteMultipleHoldingRegister(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regReadAddress;
    uint16_t regReadCount;
    uint16_t regWriteAddress;
    uint16_t regWriteCount;
    uint8_t regWriteByteCount;
    uint8_t * pFrameCur;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if (*_pLen >= (MB_PDU_FUNC_READWRITE_SIZE_MIN + MB_PDU_SIZE_MIN)) {
        regReadAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_READ_ADDR_OFF] << 8U);
        regReadAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_READ_ADDR_OFF + 1]);
        regReadAddress++;

        regReadCount = (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF] << 8U);
        regReadCount |= (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF + 1]);

        regWriteAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF] << 8U);
        regWriteAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF + 1]);
        regWriteAddress++;

        regWriteCount = (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF] << 8U);
        regWriteCount |= (uint16_t)(_pFrame[MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF + 1]);

        regWriteByteCount = _pFrame[MB_PDU_FUNC_READWRITE_BYTECNT_OFF];

        if ((regReadCount >= 1) && (regReadCount <= 0x7D)
            && (regWriteCount >= 1) && (regWriteCount <= 0x79)
                && ((2 * regWriteCount) == regWriteByteCount))
        {
            // Make callback to update the register values
            regStatus = ModbusRegHoldingCb(&_pFrame[MB_PDU_FUNC_READWRITE_WRITE_VALUES_OFF],
                                           regWriteAddress, regWriteCount, MB_REG_WRITE);

            if (regStatus == MB_ENOERR) {
                // Set the current PDU data pointer to the beginning
                pFrameCur = &_pFrame[MB_PDU_FUNC_OFF];
                *_pLen = MB_PDU_FUNC_OFF;

                // First byte contains the function code
                *pFrameCur++ = MB_FUNC_READWRITE_MULTIPLE_REGISTERS;
                *_pLen += 1;

                // Second byte in the response contain the number of bytes
                *pFrameCur++ = (uint8_t) (regReadCount * 2);
                *_pLen += 1;

                // Make the read callback
                regStatus = ModbusRegHoldingCb(pFrameCur, regReadAddress, regReadCount, MB_REG_READ);

                if (regStatus == MB_ENOERR) {
                    *_pLen += 2 * regReadCount;
                };
            };

            if (regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif